#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
// PCL specific includes
#include <pcl/ros/conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

ros::Publisher pub;
int i=0;
pcl::PointCloud<pcl::PointXYZ> cloud_in; // Temporary elaboration 
pcl::PointCloud<pcl::PointXYZ> cloud_out;

void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
{
  // ... do data processing
  sensor_msgs::PointCloud2 output;
  pcl::fromROSMsg (*input, cloud_in);
  i++;
  if(i==1){
    toROSMsg(cloud_in,output);
    cloud_out=cloud_in;
    pub.publish (output);
    return;
  }
  
  // Average Value
  for (size_t j = 0; j < cloud_in.points.size (); ++j)
  {
    cloud_out.points[j].x = cloud_out.points[j].x*(i-1)/float(i)+cloud_in.points[j].x/i;
    cloud_out.points[j].y = cloud_out.points[j].y*(i-1)/float(i)+cloud_in.points[j].y/i;
    cloud_out.points[j].z = cloud_out.points[j].z*(i-1)/float(i)+cloud_in.points[j].z/i;
  }
 
  toROSMsg(cloud_out,output);
  // Publish the data
  pub.publish (output);
}

int
main (int argc, char** argv)
{
  // Initialize ROS
  ros::init (argc, argv, "average");
  ros::NodeHandle nh;

  // Create a ROS subscriber for the input point cloud
  ros::Subscriber sub = nh.subscribe ("/stereo/points2", 1, cloud_cb);

  // Create a ROS publisher for the output point cloud
  pub = nh.advertise<sensor_msgs::PointCloud2> ("/stereo/average", 1);

  // Spin
  ros::spin ();
}


